#include <unistd.h>
#include <sys/types.h>
#include <stdio.h>

#include "libjtag.h"

static unsigned short data_port, stat_port;

/* JTAG state jump table */
int jtag_st[JTAG_STAT_COUNT][2] = {
  /*                       TMS=0          TMS=1           */
  /* Test-Logic Reset */ { RUN_TEST_IDEL, TEST_LOGIC_RESET },
  /* Run-Test/Idle    */ { RUN_TEST_IDEL, SELECT_DR_SCAN   },
  /* Select-DR-Scan   */ { CAPTURE_DR,    SELECT_IR_SCAN   },
  /* Capture-DR       */ { SHIFT_DR,      EXIT1_DR         },
  /* Shift-DR         */ { SHIFT_DR,      EXIT1_DR         },
  /* Exit1-DR         */ { PAUSE_DR,      UPDATE_DR        },
  /* Pause-DR         */ { PAUSE_DR,      EXIT2_DR         },
  /* Exit2-DR         */ { SHIFT_DR,      UPDATE_DR        },
  /* Update-DR        */ { RUN_TEST_IDEL, SELECT_DR_SCAN   },
  /* Select-IR-Scan   */ { CAPTURE_IR,    TEST_LOGIC_IDEL  },
  /* Capture-IR       */ { SHIFT_IR,      EXIT1_IR         },
  /* Shift-IR         */ { SHIFT_IR,      EXIT1_IR         },
  /* Exit1-IR         */ { PAUSE_IR,      UPDATE_IR        },
  /* Pause-IR         */ { PAUSE_IR,      EXIT2_IR         },
  /* Exit2-IR         */ { SHIFT_IR,      UPDATE_IR        },
  /* Update-IR        */ { RUN_TEST_IDEL, SELECT_DR_SCAN   },
};

/*
 * jtag_go - take jtag state machine to next state
 *
 * @tms input tms signal
 * @tdi input tdi signal
 *
 * return tdo reading.
 */
static char jtag_go(char tms, char tdi)
{
  /*
   * Local copy of data/status register for parallel port
   *
   * data: bit[3]: TDI
   *       bit[2]: TCK
   *       bit[1]: TMS
   *
   * stat: bit[7]: ~TDO
   *
   * Please refer to ciruit.txt in doc directory.
   */
  unsigned char data, stat;

  /* setup TMS */
  if (tms) {
    data |= 1<<1;
  } else {
    data &= ~(1<<1);
  }

  /* setup TDI */
  if (tdi) {
    data |= 1<<3;
  } else {
    data &= ~(1<<3);
  }

  /* first pull down TCK */
  data &= ~(1<<2);
  outb(data, data_port);

  /* TDO updated on falling edge of TCK */
  stat = inb(stat_port);

  /* then pull up TCK */
  /* TDI/TMS were sampled on rising edge of TCK */
  data |= 1<<2;
  outb(data, data_port);

  return ~(stat>>7);
}

/*
 * jtag_reset - reset jtag to Test-Logic Reset
 */
void jtag_reset(void)
{
  jtag_go(1, 1);
  jtag_go(1, 1);
  jtag_go(1, 1);
  jtag_go(1, 1);
  jtag_go(1, 1);
}

/*
 * jtag_setup - setup jtag for later use
 *
 * Jtag state will be set to Run-Test/Idel in this function.
 *
 * @dp data port for parallel port
 * @sp status port for parallel port
 *
 * return 0 on success, -1 on error.
 */
int jtag_setup(unsigned short dp, unsigned short sp)
{
  data_port = dp?dp:0x378;
  stat_port = sp?sp:0x379;

  /* sanity check */
  if (geteuid() != 0) {
    fprintf(stderr, "You don't have the permission to access I/O ports, please"
	    " ensure this executable is owned by root and have setuid bit been"
	    " setted.\n");
    return -1;
  }

#if defined(LINUX)
  /* enable port access permission */
  ioperm(data_port, 1, 1);
  ioperm(stat_port, 1, 1);
#elif defined(BSD)
  open("/dev/io"); /* man 4 io */
#endif
  
  /* drop root privilege */
  seteuid(getuid());

  /* init jtag state machine */
  jtag_reset();  /* Whatever         -> Test-Logic Reset */
  jtag_go(0, 1); /* Test-Logic Reset -> Run-Test/Idel    */
  
  return 0;
}

/*
 * jtag_shift_dr - shift data in/out data register
 *
 * Caller must ensure jtag is in Run-Test/Idel mode, and this funtion will
 * bring jtag back to Run-Test/Idel before return.
 *
 * @in   pointer to the input data bit stream
 * @out  pointer to the output data bit stream
 * @bits i/o data bit stream length
 */
void jtag_shift_dr(char *in, char *out, int bits)
{
  int i;
  char tdo;
  
  jtag_go(1, 1);                      /* Run-Test/Idel  -> Select-DR-Scan */
  jtag_go(0, 1);                      /* Select-DR-Scan -> Capture-DR     */
  jtag_go(0, 1);                      /* Capture-IR     -> Shift-DR       */

  /* shift all bits to DR, LSB first */
  for (i=0; i<bits-1; i++) {
    tdo = jtag_go(0, (in[i/8]>>(i%8))&0x01);/* Shift-DR -> Shift-DR       */
    if (tdo) {
      out[i/8] |= 1<<(i%8);
    } else {
      out[i/8] &= ~(1<<(i%8));
    }
  }
  tdo = jtag_go(1, (in[i/8]>>(i%8))&0x01);  /* Shift-DR -> Exit1-DR       */
  if (tdo) {
    out[i/8] |= 1<<(i%8);
  } else {
    out[i/8] &= ~(1<<(i%8));
  }

  jtag_go(1, 1);                      /* Exit1-DR       -> Update-DR      */
  jtag_go(0, 1);                      /* Update-DR      -> Run-Test/Idel  */
}

/*
 * jtag_shift_ir - shift instruction into instruction register
 *
 * Caller must ensure jtag is in Run-Test/Idel mode, and this funtion will
 * bring jtag back to Run-Test/Idel before return.
 *
 * @cmd  pointer to the command bit stream
 * @bits command bit stream length
 */
void jtag_shift_ir(char *cmd, int bits)
{
  int i;
  
  jtag_go(1, 1);                      /* Run-Test/Idel  -> Select-DR-Scan */
  jtag_go(1, 1);                      /* Select-DR-Scan -> Select-IR-Scan */
  jtag_go(0, 1);                      /* Select-IR-Scan -> Capture-IR     */
  jtag_go(0, 1);                      /* Capture-IR     -> Shift-IR       */

  /* shift all bits to IR, LSB first */
  for (i=0; i<bits-1; i++) {
    jtag_go(0, (cmd[i/8]>>(i%8))&0x01);     /* Shift-IR -> Shift-IR       */
  }
  jtag_go(1, (cmd[i/8]>>(i%8))&0x01);       /* Shift-IR -> Exit1-IR       */

  jtag_go(1, 1);                      /* Exit1-IR       -> Update-IR      */
  jtag_go(0, 1);                      /* Update-IR      -> Run-Test/Idel  */
}
